/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.templates.InertialReference;
import edu.wpi.first.wpilibj.templates.subsystems.DriveSubsystem;

/**
 *
 * @author matt
 */
public class Turn extends CommandBase {

    // Control constants
    final double kP = .5;
    final double kI = 0;
    final double headingTolerance = 1;
    // Operating variables
    double lastHeadingError;
    double headingError;
    double accumulatedHeadingError;
    double newHeading;
    double commandedTurn;
    boolean turnComplete = false;
    // needed classes
    InertialReference iru = null;
    DriveSubsystem.DriveMode leftPrevMode = DriveSubsystem.DriveMode.kUninitialized;
    DriveSubsystem.DriveMode rightPrevMode = DriveSubsystem.DriveMode.kUninitialized;

    public Turn(double angle) {
        // Use requires() here to declare subsystem dependencies
        requires(leftDriveSubsystem);
        requires(rightDriveSubsystem);

        commandedTurn = angle;

        System.out.println("Turn Constructed " + angle + " degrees");
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        if (null == iru) {
            iru = InertialReference.getInstance();
        }

        // figure out the new heading (relative to current heading)
        newHeading = commandedTurn + iru.getRawYaw();

        // get the old drive modes
        leftPrevMode = leftDriveSubsystem.getDriveMode();
        rightPrevMode = rightDriveSubsystem.getDriveMode();

        // set to speed mode on both sides
        leftDriveSubsystem.setSpeedMode();
        rightDriveSubsystem.setSpeedMode();

        // initialize the control loop variables
        lastHeadingError = 0;
        headingError = 0;
        accumulatedHeadingError = 0;

        turnComplete = false;
    }

    private double computeHeadingChange(double start, double finish) {
        double change = finish - start;

        // put the heading change in -180 to 180 
        while (change > 180) {
            change = change - 180;
        }
        while (change < -180) {
            change = change + 180;
        }

        return change;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        // We'll do our own PI here
        System.out.print("turning ");

        // figure out how far off on the heading we are
        headingError = computeHeadingChange(iru.getRawYaw(), newHeading);
        accumulatedHeadingError = accumulatedHeadingError + headingError;

        System.out.print(" e" + headingError);
        // reset error accumulator if we overshoot
        if (0 > headingError * lastHeadingError) {
            accumulatedHeadingError = 0;
        }

        // figure out if we're done
        if ((-headingTolerance < headingError)
                && (headingTolerance > headingError)) {
            turnComplete = true;
        } else {
            double speed = (kP * headingError) + (kI * accumulatedHeadingError);
            speed = speed/180;
            System.out.print(" s" + speed);
            if (0 < headingError) { // Turning Clockwise (from above)
                //Right Reverse
                rightDriveSubsystem.drive(-speed);
                //Left Forward
                leftDriveSubsystem.drive(+speed);
            } else {  // Turning CounterClockwise (from above)
                // Right Forward
                rightDriveSubsystem.drive(+speed);
                // Left Reverse
                leftDriveSubsystem.drive(-speed);
            }
        }
        // set up for next loop
        lastHeadingError = headingError;
System.out.println("-");
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return turnComplete;
    }

    // Called once after isFinished returns true
    protected void end() {
        if (DriveSubsystem.DriveMode.kSpeedControl == leftPrevMode) {
            leftDriveSubsystem.setSpeedMode();
        } else if (DriveSubsystem.DriveMode.kDistanceControl == leftPrevMode) {
            leftDriveSubsystem.setDistanceMode();
        }

        if (DriveSubsystem.DriveMode.kSpeedControl == rightPrevMode) {
            rightDriveSubsystem.setSpeedMode();
        } else if (DriveSubsystem.DriveMode.kDistanceControl == rightPrevMode) {
            rightDriveSubsystem.setDistanceMode();
        }
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        // set the drive mode back to what is expected if we're interrupted
        end();
    }
}
